%%% Parameters
tf = 3.3155; r0 = 1;  T=0.1405; mdot=.07489;
narr = [20 100]; % Array with consecutive number of collocation points
%%% Initial state
xi=[r0; 0; sqrt(1/r0)];
for n = narr
  toms t;
  p = tomPhase('p', t, 0, tf, n);
  setPhase(p)
  tomStates r u v
  tomControls theta
  %%% Initial guess
  if n==narr(1)
    x0 = {icollocate({r == xi(1); u == xi(2); v == xi(3)})
      collocate({theta == 0})};
  else
    x0 = {icollocate({r == xopt1; u == xopt2; v == xopt3})
      collocate({theta == uopt1})};
  end  
  %%% Constraints
  cbnd = {initial({
    r == xi(1); 
    u == xi(2); 
    v == xi(3)})
    final({
    u == 0; 
    v-1/sqrt(r) == 0})};
  ceq = collocate({
    dot(r) == u
    dot(u) == (v^2)/r-1/(r^2)+T/(1-mdot*t)*sin(theta)
    dot(v) == -u*v/r+T/(1-mdot*t)*cos(theta)
    });
  %%% Objective
  objective = -final(r);
  %%% Solve problem
  options = struct;
  options.name = 'maxRadiusOrbitTransfer';
  solution = ezsolve(objective, {cbnd, ceq}, x0, options);
  xopt1 = subs(r,solution);
  xopt2 = subs(u,solution);
  xopt3 = subs(v,solution);
  uopt1 = subs(theta,solution);
end
%%% Show result
tSol = subs(collocate(t),solution);
rSol = subs(collocate(r),solution);
uSol = subs(collocate(u),solution);
vSol = subs(collocate(v),solution);
thetaSol = subs(collocate(theta),solution);
thetaSol = mod(thetaSol, 2*pi);
finalRadius = rSol(end);
fprintf('Final radius: %f \n', finalRadius)
figure
subplot(2,1,1), plot(tSol,rSol,'.-',tSol,uSol,'.-',tSol,vSol,'.-'); 
legend('r','u','v'); title('states trajectories');
subplot(2,1,2), plot(tSol,thetaSol,'+-'); legend('\theta');
title('control signal');